#include <iostream>
#include <SFML/Graphics.hpp>
#include <math.h>
#include <queue>
#include <vector>

using namespace std;

template<class T>
struct vec2
{
    T x, y;
};

bool operator== (const vec2<int>& lhs, const vec2<int>& rhs){ return lhs.x == rhs.x && lhs.y == rhs.y; }

template<class T>
vec2<float> regress(std::vector<vec2<T>> p)
{
    float xm=0;
    float ym=0;

    for(auto& i: p)
    {
        ym += i.y;
        xm += i.x;
    }

    xm/=p.size();
    ym/=p.size();

    float tl=0;
    float tb=0;

    for(auto& i: p)
    {
        float xd = i.x - xm;
        float yd = i.y - ym;

        tl+=(xd - xm)*(yd - ym);
        tb+=(xd - xm)*(xd - xm);
    }

    float b = tl / tb;
    float a = ym - b*xm;

    return {a, b};
}

/*template<class T>
vec2<float> regress(std::vector<vec2<T>> p)
{
    float sx=0, sy=0, sx2=0, sxy=0;

    for(auto& i : p)
    {
        sx+=i.x;
        sy+=i.y;
        sx2+=i.x*i.x;
        sxy+=i.x*i.y;
    }

    int n = p.size();

    float a = (sy*sx2 - sx*sxy) / (n*sx2 - sx*sx);
    float b = (n*sxy - sx*sy) / (n*sx2 - sx*sx);


    return {a, b};
}*/

void prepass(sf::Image &img, float bth)
{
    float colour_mag_threshold = 10;
    float transp_threshold = 0;
    float transp_upper_threshold = 255;

    float transp_middle_default = 150;

    sf::Image b = img;

    sf::Color black(0,0,0,255);
    sf::Color white(255,255,255,255);
    sf::Color midblacktransp(128, 128, 128, 255);

    for(int y=0; y<img.getSize().y; y++)
    {
        for(int x=0; x<img.getSize().x; x++)
        {
            sf::Color col = b.getPixel(x, y);




            float rd, gd, bd;

            rd = (white.r - col.r);
            gd = (white.g - col.g);
            bd = (white.b - col.b);

            float wdist = sqrt(rd*rd + gd*gd + bd*bd);

            rd = (black.r - col.r);
            gd = (black.g - col.g);
            bd = (black.b - col.b);

            float bdist = sqrt(rd*rd + gd*gd + bd*bd);


            if(wdist < 50)
            {
                img.setPixel(x, y, white);
                continue;
            }

            else if(bdist < bth)
            {
                img.setPixel(x, y, black);
                continue;
            }

            else
            {
                bool d1 = false;
                /*if(y-1 > 0)
                {
                    sf::Color com = b.getPixel(x, y-1);
                    int r, g, b;
                    r = com.r + col.r;
                    g = com.g + col.g;
                    b = com.b + col.b;

                    float m1 = sqrt(com.r*com.r + com.g*com.g + com.b*com.b);
                    float m2 = sqrt(col.r*col.r + col.g*col.g + col.b*col.b);

                    if(sqrt(r*r + g*g + b*b) < bth*4 && m1 > bth && m2 > bth)
                    {
                        img.setPixel(x, y, sf::Color(0, 0, 0));
                        d1 = true;
                    }
                    else
                    {
                        img.setPixel(x, y, midblacktransp);
                    }
                }
                if(x-1 > 0)
                {
                    sf::Color com = b.getPixel(x-1, y);
                    int r, g, b;
                    r = com.r + col.r;
                    g = com.g + col.g;
                    b = com.b + col.b;

                    float m1 = sqrt(com.r*com.r + com.g*com.g + com.b*com.b);
                    float m2 = sqrt(col.r*col.r + col.g*col.g + col.b*col.b);

                    if(sqrt(r*r + g*g + b*b) < bth*3 && m1 > bth && m2 > bth)
                    {
                        d1 = true;
                        img.setPixel(x, y, sf::Color(0, 0, 0));
                    }
                    else if(!d1)
                    {
                        img.setPixel(x, y, midblacktransp);
                    }
                }*/
                if(!d1)
                {
                    img.setPixel(x, y, midblacktransp);
                }
                //
            }
        }
    }
}

template<class T>
bool is_within(std::vector<vec2<T>>& l, vec2<T>& t)
{
    for(int i=0; i<l.size(); i++)
    {
        if(t.x == l[i].x && t.y == l[i].y)
        {
            return true;
        }
    }
    return false;
}

struct distpos
{
    float d, x, y;
};

bool operator< (const distpos& lhs, const distpos& rhs){ return lhs.d < rhs.d; }


///calculates the sqrt(sum squared distance from expected value given regression)

template<class T>
float deviation_ish(std::vector<vec2<T>>& vec)
{
    vec2<float> regr = regress(vec);

    float totaldist = 0;

    for(auto i : vec)
    {
        float cy = regr.y*(i.x) + regr.x;
        float dist = i.y - cy;
        dist = dist*dist;
        totaldist += dist;
    }

    totaldist /= vec.size();

    //totaldist = sqrt(totaldist);

    return totaldist;
}

std::vector<vec2<int>> line_remove(std::vector<vec2<int>>** memoise, sf::Image& img, float fault_tolerance, int step, int cx, int cy, int n=0, std::vector<vec2<int>> vec = std::vector<vec2<int>>())
{
    //int ix = 6;
    //int iy = img.getSize().y - 4;

    int ix = cx;
    int iy = cy;

    //int ix = img.getSize().x - 5;
    //int iy = img.getSize().y - 5;

    //img.setPixel(ix, iy, sf::Color(255, 0, 0));

    std::vector<vec2<int>> points = vec;

    if(points.size() == 0)
        points.push_back({(int)ix, (int)iy});

    int chopsize = 20;

    if(n < step || step == 0)
    {
        std::vector<vec2<int>> chop;
        if(points.size() > chopsize)
        {
            for(int i = points.size() - chopsize; i < points.size(); i++)
            {
                chop.push_back(points[i]);
            }
        }

        vec2<float> regr;

        if(points.size() > chopsize)
            regr = regress(chop);
        else
            regr = regress(points);

        vec2<int> cur = points.back();

        float bdist = 1000;
        int nx = -2;
        int ny = -2;

        std::vector<distpos> p;

        for(int y=-1; y<2; y++)
        {
            for(int x=-1; x<2; x++)
            {
                if(x == 0 && y == 0)
                    continue;

                vec2<int> f = {cur.x + x, cur.y + y};

                if(is_within(points, f))
                {
                    continue;
                }

                sf::Color col = img.getPixel(cur.x + x, cur.y + y);

                if(col.r != 0 || col.g != 0 || col.b != 0)
                {
                    continue;
                }

                float cy = regr.y*(x + cur.x) + regr.x;
                float dist = fabs((cur.y + y)) - fabs(cy);

                if(n == step-1)
                {
                    //std::cout << cy << " " << dist << std::endl << endl;
                }

                p.push_back({dist, (int)x, (int)y});
            }
        }

        for(auto& k : p)
        {
            if(k.d < bdist)
            {
                bdist = k.d;
                nx = k.x;
                ny = k.y;
            }
        }

        ///end/base case
        if(nx == -2)
        {
            return points;
        }

        std::vector<distpos> consider;

        float tol = fault_tolerance;

        for(auto& k : p)
        {
            if(k.d < bdist + tol && k.d > bdist - tol)
            {
                consider.push_back({k.d, k.x, k.y});
            }
        }

        std::vector<std::vector<vec2<int>>> results;

        for(auto& k : consider)
        {
            distpos c = k;
            c.x += cur.x;
            c.y += cur.y;

            auto tpoints = points;
            tpoints.push_back({c.x, c.y});

            sf::Image t = img;

            auto& mem = memoise[(int)c.y][(int)c.x];

            if(mem.size()==0)
            {
                auto npoints = line_remove(memoise, t, fault_tolerance, step, cx, cy, n+1, tpoints);

                results.push_back(npoints);

                memoise[(int)c.y][(int)c.x] = npoints;
            }
            else
            {
                auto npoints = mem;

                results.push_back(npoints);
            }
        }

        std::vector<pair<float, int>> deviations;
        std::vector<pair<float, int>> lens;

        for(int n=0; n<results.size(); n++)
        {
            std::vector<vec2<int>> k = results[n];
            float dev = deviation_ish(k);
            deviations.push_back({dev, n});
            lens.push_back({k.size(), n});
        }

        std::vector<pair<distpos, int>> cid;

        int a=0;
        for(auto i: consider)
        {
            cid.push_back({{i.d, i.x, i.y}, a});
            a++;
        }

        std::sort(cid.begin(), cid.end());

        //std::sort(deviations.begin(), deviations.end());
        std::sort(lens.begin(), lens.end());

        int longest = lens.back().second;
        int straightest = deviations.back().second;

        bool is_one = false;

        for(int i=lens.size()-1; i>=0; i--)
        {
            if(deviations[lens[i].second].first < tol)
            {
                longest = lens[i].second;
                is_one = true;
                break;
            }
        }

        if(!is_one)
        {
            std::sort(deviations.begin(), deviations.end());
            return results[deviations.back().second];
        }

        return results[longest];
    }

    return points;
}

class image_iterator : public std::iterator<std::input_iterator_tag, int>
{
    public:
    int x, y;
    sf::Image* img;
    image_iterator(int px, int py, sf::Image* pimg) : x(px), y(py), img(pimg) {};
    image_iterator(const image_iterator& it) : x(it.x), y(it.y) {}
    image_iterator& operator++() {x++; if(x == img->getSize().x) {x = 0; y++;}}
    image_iterator operator++(int) {image_iterator it(*this); operator++(); return it;}
    sf::Color operator*() {return img->getPixel(x, y);}
    bool operator==(const image_iterator& rhs) {return rhs.x == x && rhs.y == y;}
    bool operator!=(const image_iterator& rhs) {return !(rhs.x == x && rhs.y == y);}
};

struct it_image
{
    sf::Image* img;
    image_iterator begin() {return {0,0,img};}
    image_iterator end() {return {0,img->getSize().y+1,img};}
};

bool within_bounds(int i, int j, sf::Image* img)
{

    if(i<0 || j<0)
        return false;

    if(i>img->getSize().x || j > img->getSize().y)
        return false;

    return true;
}


bool is_col(sf::Color, sf::Color, int);

///??? starting direction ???
std::vector<vec2<int>> straight_line_detect(std::vector<vec2<int>>** memoise, sf::Image& img, float fault_tolerance, int step, int cx, int cy, int n=0, std::vector<vec2<int>> vec = std::vector<vec2<int>>())
{
    cout << step << endl;
    if(vec.size()==0)
    {
        vec.push_back({cx, cy});
        std::vector<std::vector<vec2<int>>> ret;
        for(int i=-1; i<2; i++)
        {
            for(int j=-1; j<2; j++)
            {
                if(!within_bounds(i+cx, j+cy, &img))
                    continue;

                if(i==0 && j==0)
                    continue;

                sf::Color col = img.getPixel(cx+i, cy+j);

                if(col.r > 30 || col.g > 30 || col.b > 30)
                    continue;

                ret.push_back(straight_line_detect(memoise, img, fault_tolerance, step+1, 0, 0, 0, {{cx, cy}, {i+cx, j+cy}}));
            }
        }

        int ml=0, mid = 0;
        int n = 0;
        for(auto& i : ret)
        {
            if(i.size() > ml)
            {
                ml = i.size();
                mid = n;
            }
            n++;
        }

        if(ret.size() > 0)
            return ret[mid];

        return {{cx, cy}};
    }
    else
    {
        vec2<int> point = vec.back();


        std::vector<distpos> possible;


        int x = vec.back().x;
        int y = vec.back().y;

        vec2<float> regr = regress(vec);

        for(int i=-1; i<2; i++)
        {
            for(int j=-1; j<2; j++)
            {
                ///line of regression not very good

                if(!within_bounds(i+x, y+j, &img))
                    continue;

                if(i==0 && j==0)
                    continue;

                sf::Color col = img.getPixel(x+i, y+j);

                if(col.r > 30 || col.g > 30 || col.b > 30)
                    continue;

                float expected_y = regr.y*(x + i) + regr.x;
                float dist = (y + j) - expected_y;
                dist = dist*dist;

                vec2<int> v = {x+i, y+j};

                if(dist < fault_tolerance*fault_tolerance*3 && !is_within(vec, v))
                {
                    possible.push_back({dist, x+i, y+j});
                    cout << x+i << " " << y+j << endl;
                }
            }
        }

        vector<vector<vec2<int>>> stor;
        if(possible.size() > 0)
        {
            int most = 0;
            int id = 0;
            int c = 0;

            for(auto& res: possible)
            {
                vector<vec2<int>> t = vec;
                t.push_back({res.x, res.y});

                auto& mem = memoise[t.back().y][t.back().x];

                if(mem.size()==0)
                {
                    stor.push_back(straight_line_detect(memoise, img, fault_tolerance, step+1, 0, 0, 0, t));
                    mem = stor.back();
                }
                else
                {
                    stor.push_back(mem);
                }

                if(stor.back().size() > most)
                {
                    most = stor.back().size();
                    id = c;
                }

                c++;
            }

            return stor[id];

        }
        else
        {
            return {vec.back()};
        }

    }
}

sf::Color line_col;

void process_line_remove(sf::Image& img, std::vector<vec2<int>> points)
{

    sf::Color black(0,0,0);
    sf::Color white(255,255,255);

    line_col = sf::Color(200, 200, 200);

    sf::Image t = img;

    for(auto& i : points)
    {
        int sides[4] = {0};
        int x0 = 0;
        int y0 = 0;

        for(int y=-1; y<2; y++)
        {
            for(int x=-1; x<2; x++)
            {
                if(x==0 && y==0)
                {
                    continue;
                }

                int px, py;
                px = i.x + x;
                py = i.y + y;

                sf::Color col = t.getPixel(px, py);
                if(col.r < 30 && col.g < 30 && col.b < 30)
                {
                    if(x<0)
                    {
                        sides[3]++;
                    }
                    if(x>0)
                    {
                        sides[1]++;
                    }
                    if(y<0)
                    {
                        sides[0]++;
                    }
                    if(y>0)
                    {
                        sides[2]++;
                    }
                    if(y==0)
                        y0++;
                    if(x==0)
                        x0++;
                }
            }
        }

        ///do if black bit is also line

        ///need to remove if the case where its on a thing with the things where it sticks out, ie all side x but both sides y

        if(sides[1] && sides[3] && y0)
        {
            img.setPixel(i.x, i.y, black);
        }

        else if(sides[2] && sides[0] && x0)
        {
            img.setPixel(i.x, i.y, black);
        }
        else// if(img.getPixel(i.x,i.y).r<30 && img.getPixel(i.x,i.y).g<30 && img.getPixel(i.x,i.y).b<30)
        {
            img.setPixel(i.x, i.y, line_col);
        }

    }
}

std::vector<vec2<int>> line_optimise_detect(std::vector<vec2<int>>** memoise, sf::Image& img, float f_t, int cx, int cy)
{
    for(int j=0; j<img.getSize().y; j++)
    {
        for(int i=0; i<img.getSize().x; i++)
        {
            memoise[j][i].clear();
        }
    }


    std::vector<vec2<int>> v = line_remove(memoise, img, f_t, 0, cx, cy);
    std::vector<vec2<int>> t = v;

    vec2<int> e = v.back();

    while(t.back().x == e.x && t.back().y == e.y)
    {
        for(int j=0; j<img.getSize().y; j++)
        {
            for(int i=0; i<img.getSize().x; i++)
            {
                memoise[j][i].clear();
            }
        }

        sf::Image img3 = img;

        v = t;
        if(f_t - 0.1 <= 0.1)
        {
            break;
        }

        t = line_remove(memoise, img3, f_t - 0.1, 0, cx, cy);
        f_t-=0.1;
        img = img3;
    }

    for(auto& n : v)
    {
        //img.setPixel(n.x, n.y, sf::Color(255, 0, 0));
    }
    return v;
}

void remove_artifacts(sf::Image& img)
{
    sf::Color black(0,0,0);
    sf::Color white(255,255,255);
    sf::Color grey(128,128,128);

    for(int y=0; y<img.getSize().y; y++)
    {
        for(int x=0; x<img.getSize().x; x++)
        {
            int any_white = 0;
            int any_black = 0;
            int is_black=0;
            int is_grey=0;
            int all_white = 1;

            sf::Color t = img.getPixel(x, y);

            if(t==black)
            {
                is_black=1;
            }
            if(t==grey)
            {
                is_grey=1;
            }

            for(int j=-1; j<2; j++)
            {
                for(int i=-1; i<2; i++)
                {
                    if(i==0 && j==0)
                    {
                        continue;
                    }
                    if(y+j < 0 || x+i < 0 || y+j>=img.getSize().y || x+i>=img.getSize().x)
                    {
                        continue;
                    }

                    sf::Color col = img.getPixel(x+i, y+j);

                    if(col == white)
                    {
                        any_white = 1;
                    }
                    else
                    {
                        all_white = 0;
                    }
                    if(col == black)
                    {
                        any_black = 1;
                    }

                }
            }
            if(is_black && any_white && !any_black)
            {
                //img.setPixel(x, y, sf::Color(255, 128, 128));
                img.setPixel(x, y, grey);
                x-=1;
                if(x<0)
                    y--;
                continue;
            }
            if(is_grey && !any_black)
            {
                ///img.setPixel(x, y, sf::Color(128, 255, 128));
                //img.setPixel(x, y, white);
                //x-=1;
                //if(x<0)
                //    y--;
            }
            if(is_grey && !any_black && all_white)
            {
                img.setPixel(x, y, white);
                x-=1;
                if(x<0)
                    y--;
            }
        }
    }
}

void remove_line_extras(sf::Image& img, std::vector<vec2<int>> points)
{
    sf::Color black(0,0,0);
    sf::Color white(255,255,255);
    sf::Color grey(128,128,128);

    for(auto& k : points)
    {

        sf::Color c = img.getPixel(k.x, k.y);

        for(int j=-1; j<2; j++)
        {
            for(int i=-1; i<2; i++)
            {
                if(i==0 && j==0)
                {
                    continue;
                }
                if(k.y+j < 0 || k.x+i < 0 || k.y+j>=img.getSize().y || k.x+i>=img.getSize().x)
                {
                    continue;
                }

                sf::Color col = img.getPixel(k.x+i, k.y+j);
            }
        }

        if(c == line_col)
        {
            for(int j=-1; j<2; j++)
            {
                for(int i=-1; i<2; i++)
                {
                    if(i==0 && j==0)
                    {
                        continue;
                    }
                    if(k.y+j < 0 || k.x+i < 0 || k.y+j>=img.getSize().y || k.x+i>=img.getSize().x)
                    {
                        continue;
                    }
                    sf::Color col = img.getPixel(k.x+i, k.y+j);
                    if(col == grey)
                    {
                        img.setPixel(k.x+i, k.y+j, white);
                    }
                }
            }
            img.setPixel(k.x, k.y, grey);
        }
    }
}

void restore_original(sf::Image& proc, sf::Image& o, std::vector<std::vector<vec2<int>>> lines)
{
    sf::Color black(0,0,0);
    sf::Color white(255,255,255);
    sf::Color grey(128,128,128);

    for(int x=0; x<proc.getSize().x; x++)
    {
        for(int y=0; y<proc.getSize().y; y++)
        {
            sf::Color col = proc.getPixel(x, y);

            if(col == black || col == grey)
            {
                bool not_removed = true;

                for(auto &i : lines)
                {
                    vec2<int> l = {(int)x, (int)y};
                    if(is_within(i, l))
                    {
                        not_removed = false;
                    }
                }

                if(not_removed && ((col == grey && o.getPixel(x, y).r > 20 && o.getPixel(x,y).g > 20 && o.getPixel(x,y).b > 20) || col == black))
                    proc.setPixel(x, y, o.getPixel(x, y));
            }

        }
    }


}

void restore_more_AA(sf::Image& proc, sf::Image& o, std::vector<std::vector<vec2<int>>> lines)
{
    for(int x=0; x<proc.getSize().x; x++)
    {
        for(int y=0; y<proc.getSize().y; y++)
        {

            sf::Color col = proc.getPixel(x, y);

            for(int j=-1; j<2; j++)
            {
                for(int i=-1; i<2; i++)
                {
                    if(i==0 && j==0)
                    {
                        continue;
                    }
                    if(y+j < 0 || x+i < 0 || y+j>=proc.getSize().y || x+i>=proc.getSize().x)
                    {
                        continue;
                    }

                    bool not_removed = true;

                    for(auto &n : lines)
                    {
                        vec2<int> l = {x + i, y + j};
                        if(is_within(n, l))
                        {
                            not_removed = false;
                        }
                    }

                    if(col.r < 30 && col.g < 30 && col.b < 30 && proc.getPixel(x+i, y+j).r > 230 && proc.getPixel(x+i, y+j).g > 230 && proc.getPixel(x+i, y+j).b > 230)
                    {
                        //proc.setPixel(x+i, y+j, o.getPixel(x+i, y+j));
                        sf::Color t = o.getPixel(x+i, y+j);
                        if(t.r < 30 && t.g < 30 && t.b < 30)
                        {
                            t = sf::Color(128, 128, 128);
                        }
                        if(not_removed)
                        {
                            //t = sf::Color(200, 0, 0);
                        }
                        proc.setPixel(x+i, y+j, t);
                    }

                }
            }



        }
    }
}

void remove_single_black(sf::Image& proc)
{
    sf::Color black(0,0,0);
    sf::Color white(255,255,255);
    sf::Color grey(128,128,128);

    for(int x=0; x<proc.getSize().x; x++)
    {
        for(int y=0; y<proc.getSize().y; y++)
        {
            sf::Color col = proc.getPixel(x, y);
            bool repl = true;

            for(int j=-1; j<2; j++)
            {
                for(int i=-1; i<2; i++)
                {
                    if(i==0 && j==0)
                    {
                        continue;
                    }
                    if(y+j < 0 || x+i < 0 || y+j>=proc.getSize().y || x+i>=proc.getSize().x)
                    {
                        continue;
                    }
                    sf::Color col = proc.getPixel(x+i, y+j);
                    if(col != white && col!=grey)
                    {
                        repl = false;
                    }
                }
            }

            if(repl)
            {
                proc.setPixel(x, y, white);
            }
        }
    }
}

pair<vec2<int>, float> get_min(std::vector<pair<vec2<int>, float>> v)
{
    int val = 1000000;
    int n = -1;

    int k = 0;

    for(auto& i : v)
    {
        if(i.second < val)
        {
            n = k;
            val = i.second;
        }
        k++;
    }

    return v[n];
}

void rec_p(sf::Image& img, vec2<int>* came_from, vec2<int> node, std::vector<vec2<int>>& path)
{
    vec2<int> next = came_from[node.x + img.getSize().x*node.y];
    img.setPixel(node.x, node.y, sf::Color(255, 0, 255));
    path.push_back(node);
    if(next.x > 0)
    {
        rec_p(img, came_from, next, path);
    }
}

vec2<int> get_end(std::vector<vec2<int>>& line)
{
    vec2<int> start = line.front();
    int ydir = line.back().y - line.front().y;
    int xdir = line.back().x - line.front().x;

    if(ydir == 0)
        ydir = 1;
    if(xdir == 0)
        xdir = 1;
    ydir = ydir/abs(ydir);
    xdir = xdir/abs(xdir);

    vec2<int> c_end = line.front();

    for(auto& i : line)
    {
        if(i.x*xdir > c_end.x*xdir)
        {
            c_end = i;
        }
        else if(i.y*ydir > c_end.y*ydir)
        {
            c_end = i;
        }
    }

    return c_end;
}

std::vector<vec2<int>> dijkstra(sf::Image& img, std::vector<std::vector<vec2<int>>> lines, int lnum)
{
    sf::Color red(255,0,0);
    sf::Color black(0,0,0);

    vec2<int> start = lines[lnum].front();
    vec2<int> stop = get_end(lines[lnum]);

    std::vector<vec2<int>> closed;
    std::vector<pair<vec2<int>, float>> open; ///with g val
    open.push_back({start, 0});

    int width = img.getSize().x;
    vec2<int>* came_from = new vec2<int>[img.getSize().y*img.getSize().x];

    came_from[0] = {-1, -1};

    while(open.size()!=0)
    {
        auto cur = get_min(open);
        if(cur.first.x == stop.x && cur.first.y == stop.y)
        {
            std::vector<vec2<int>> path;
            rec_p(img, came_from, cur.first, path);
            return path;
        }

        auto rem = std::find(open.begin(), open.end(), cur);
        open.erase(rem);

        closed.push_back(cur.first);

        for(int j=-1; j<2; j++)
        {
            for(int i=-1; i<2; i++)
            {
                if(i==0 && j==0)
                {
                    continue;
                }
                if(cur.first.y+j < 0 || cur.first.x+i < 0 || cur.first.y+j>=img.getSize().y || cur.first.x+i>=img.getSize().x)
                {
                    continue;
                }
                if(img.getPixel(cur.first.x+i, cur.first.y+j)!=black && img.getPixel(cur.first.x+i, cur.first.y+j)!=red)
                {
                    continue;
                }

                vec2<int> neigh = {cur.first.x + i, cur.first.y + j};

                if(std::find(closed.begin(), closed.end(), neigh)!=closed.end())
                {
                    continue;
                }

                float tfs = cur.second + sqrt(fabs(i) + fabs(j));

                bool in_openset = false;

                float f = -1;

                pair<vec2<int>, float> *v = NULL;

                for(auto& l: open)
                {
                    if(l.first.x == neigh.x && l.first.y == neigh.y)
                    {
                        in_openset = true;
                        f = l.second;
                        v = &l;
                        break;
                    }
                }

                if(!in_openset || tfs < v->second)
                {
                    float val = tfs;
                    came_from[neigh.x + width*neigh.y] = cur.first;


                    if(!in_openset)
                    {
                        open.push_back({neigh, val});
                    }
                    else
                    {
                        v->second = val;
                    }
                }


            }
        }


    }
}

bool is_col(sf::Color col, sf::Color colcheck, int tol)
{
    float rd, gd, bd;
    rd = col.r - colcheck.r;
    gd = col.g - colcheck.g;
    bd = col.b - colcheck.b;
    float mag = sqrt(rd*rd + gd*gd + bd*bd);
    return mag < (float)tol;
}



///takes in a non prepassed image
/*void om_nom_nom_AA(sf::Image& img, int tol)
{
    sf::Image b = img;

    for(int x=0; x<b.getSize().x; x++)
    {
        for(int y=0; y<b.getSize().y; y++)
        {
            if(y-1 < 0)
            {
                continue;
            }

            sf::Color up = b.getPixel(x, y-1);
            sf::Color cur = b.getPixel(x, y);

            if(is_col(up, sf::Color(0,0,0), tol) || is_col(cur, sf::Color(0,0,0), tol))
            {
                continue;
            }

            if(cur.r > 170 && cur.g > 170 && cur.b > 170 || up.r > 170 && up.g > 170 && up.b > 170)
            {
                continue;
            }

            float rs, gs, bs;
            rs = up.r + cur.r;
            gs = up.g + cur.g;
            bs = up.b + cur.b;

            float mag = sqrt(rs*rs + gs*gs + bs*bs);

            ///also check all surrounding pixels are not black

            int any_black = 0;

            for(int j=-1; j<2; j++)
            {
                for(int i=-1; i<2; i++)
                {
                    if(i==0 && j==0)
                    {
                        continue;
                    }
                    if(y+j < 0 || x+i < 0 || y+j>=img.getSize().y || x+i>=img.getSize().x)
                    {
                        continue;
                    }

                    if(is_col(img.getPixel(x+i, y+j), sf::Color(0,0,0), tol))
                    {
                        any_black = 1;
                    }
                }
            }



            if(mag < tol*5.5f && !any_black)
            {
                img.setPixel(x, y, sf::Color(0,0,0));
            }

        }
    }

    for(int x=0; x<b.getSize().x; x++)
    {
        for(int y=0; y<b.getSize().y; y++)
        {
            if(x-1 < 0)
            {
                continue;
            }

            sf::Color up = b.getPixel(x-1, y);
            sf::Color cur = b.getPixel(x, y);

            if(is_col(up, sf::Color(0,0,0), tol) || is_col(cur, sf::Color(0,0,0), tol))
            {
                continue;
            }

            if(cur.r > 170 && cur.g > 170 && cur.b > 170 || up.r > 170 && up.g > 170 && up.b > 170)
            {
                continue;
            }

            float rs, gs, bs;
            rs = up.r + cur.r;
            gs = up.g + cur.g;
            bs = up.b + cur.b;

            float mag = sqrt(rs*rs + gs*gs + bs*bs);

            ///also check all surrounding pixels are not black

            int any_black = 0;

            for(int j=-1; j<2; j++)
            {
                for(int i=-1; i<2; i++)
                {
                    if(i==0 && j==0)
                    {
                        continue;
                    }
                    if(y+j < 0 || x+i < 0 || y+j>=img.getSize().y || x+i>=img.getSize().x)
                    {
                        continue;
                    }

                    if(is_col(img.getPixel(x+i, y+j), sf::Color(0,0,0), tol))
                    {
                        any_black = 1;
                    }
                }
            }



            if(mag < tol*6.5f && !any_black)
            {
                img.setPixel(x, y, sf::Color(0,0,0));
            }

        }
    }
}*/

sf::Color flood_filled = sf::Color(0, 0, 254);

pair<vec2<int>, vec2<int>> flood_fill_aggressively(sf::Image& img, int x, int y, int id)
{
    vec2<int> st = {x, y};
    vec2<int> en = st;

    img.setPixel(x, y, sf::Color(id, 0, 254));
    for(int i=-1; i<2; i++)
    {
        for(int j=-1; j<2; j++)
        {
            if(i==0 && j==0)
            {
                continue;
            }
            if(y+j < 0 || x+i < 0 || y+j>=img.getSize().y || x+i>=img.getSize().x)
            {
                continue;
            }

            if(is_col(img.getPixel(x+i, y+j), sf::Color(0,0,0), 90))
            {
                auto bb = flood_fill_aggressively(img, x+i, y+j, id);
                if(bb.second.x > en.x)
                {
                    //en = bb.second;
                    en.x = bb.second.x;
                }
                if(bb.second.y > en.y)
                {
                    en.y = bb.second.y;
                }
                if(bb.first.x < st.x)
                {
                    st.x = bb.first.x;
                }
                if(bb.first.y < st.y)
                {
                    st.y = bb.first.y;
                }
            }

        }
    }

    return {st, en};
}


///floodfill black to find separate glyphs

std::vector<sf::Image> glyphiphy(sf::Image& img)
{
    sf::Image pass = img;
    prepass(pass, 100);

    vector<pair<vec2<int>,vec2<int>>> bbs;

    std::vector<sf::Image> in;

    for(int x=0; x<img.getSize().x; x++)
    {
        for(int y=0; y<img.getSize().y; y++)
        {
            if(is_col(img.getPixel(x, y), sf::Color(0,0,0), 90))
            {
                bbs.push_back(flood_fill_aggressively(img, x, y, bbs.size()));
            }
        }
    }

    int cid = 0;
    for(auto& i : bbs)
    {
        vec2<int> st = i.first;
        vec2<int> en = i.second;

        sf::Image im;
        im.create(en.x - st.x + 1, en.y - st.y + 1);
        //im.copy(img, 0, 0, sf::IntRect(st.x, st.y, en.x, en.y));

        for(int x=0; x<en.x - st.x + 1; x++)
        {
            for(int y=0; y<en.y - st.y + 1; y++)
            {
                sf::Color col = img.getPixel(x + st.x, y + st.y);
                int idt = col.r;
                if(idt == cid)
                    im.setPixel(x, y, col);
            }
        }

        in.push_back(im);
        cid++;
    }

    return in;
}

int scale = 5;

void draw(sf::RenderWindow &win, sf::Image& img, int x, int y)
{
    sf::Texture tex;
    tex.loadFromImage(img);
    sf::Sprite spr(tex);
    spr.scale(scale, scale);
    spr.setPosition(x, y);

    win.draw(spr);
}

int main()
{


    sf::RenderWindow win(sf::VideoMode(800,600), "test");

    sf::Image crop2;
    crop2.loadFromFile("Crop2.jpg");

    sf::Image img;
    img.loadFromFile("Crop.jpg");

    sf::Image imgo;
    imgo = img;

    prepass(img, 90);

    sf::Image rt = img;

    sf::Texture tex;

    sf::Sprite spr;
    spr.scale(scale, scale);

    win.draw(spr);

    tex.loadFromImage(imgo);
    spr.setTexture(tex);
    spr.setPosition(0, imgo.getSize().y*scale + scale);
    win.draw(spr);

    win.display();

    win.draw(spr);

    sf::Event event;

    int istep = 1;

    std::vector<vec2<int>>** memoise = new std::vector<vec2<int>>*[img.getSize().y];
    for(int i=0; i<img.getSize().y; i++)
    {
        memoise[i] = new std::vector<vec2<int>>[img.getSize().x];
    }

    sf::Image img2 = img;

    auto v = line_optimise_detect(memoise, img2, 0.5, 6, img2.getSize().y - 4);
    auto v2 = line_optimise_detect(memoise, img2, 1.4, img2.getSize().x-5, img2.getSize().y - 5);

    //for(int i=0; i<2; i++)
    //    dijkstra(img2, {v, v2}, i);

    v = dijkstra(img2, {v, v2}, 0);
    v2 = dijkstra(img2, {v, v2}, 1);


    //img2.setPixel(v2.back().x, v2.back().y, sf::Color(255, 0, 0));

    //std::vector<vec2<float>> purple_line_remove; ///scratch that, get dijkstras to return the new line through rec_p


    sf::Image post_line_detect = img2;

    sf::Image more_black = imgo;

    //prepass(more_black, 90);

    sf::Image t3 = img2;

    process_line_remove(img2, v);
    process_line_remove(img2, v2);

    remove_line_extras(img2, v);
    remove_line_extras(img2, v2);

    remove_artifacts(img2);
    remove_artifacts(img2);

    restore_original(img2, imgo, {v, v2});

    remove_single_black(img2);

    restore_more_AA(img2, imgo, {v, v2});
    restore_more_AA(img2, imgo, {v, v2});
    restore_more_AA(img2, imgo, {v, v2});



    //om_nom_nom_AA(img2, 90);

    //process_line_remove(img2, v);
    //process_line_remove(img2, v2);

    //restore_guess_AA(img2, imgo);

    img2.saveToFile("Output.png");


    auto imgvec = glyphiphy(img2);

    int xacc = 0;

    for(auto& i: imgvec)
    {
        draw(win, i, xacc*scale, 0);
        win.display();
        draw(win, i, xacc*scale, 0);
        xacc += i.getSize().x + 5;
    }

    for(int i=0; i<img2.getSize().x; i++)
    {
        for(int j=0; j<img2.getSize().y; j++)
        {
            if(img2.getPixel(i, j).b==254 && img2.getPixel(i, j).g == 0)
            {
                img2.setPixel(i,j,sf::Color(0,0,0));
            }
        }
    }


    sf::Image temp = img2;

    draw(win, img2, img2.getSize().x*scale, img2.getSize().y*scale + scale);
    win.display();
    draw(win, img2, img2.getSize().x*scale, img2.getSize().y*scale + scale);

    draw(win, post_line_detect, 0, post_line_detect.getSize().y*scale*2);
    win.display();
    draw(win, post_line_detect, 0, post_line_detect.getSize().y*scale*2);


    sf::Mouse mouse;

    while(win.isOpen())
    {
        while(win.pollEvent(event))
        {
            if(event.type == sf::Event::Closed)
            {
                win.close();
                break;
            }
        }

        //while(event.type != sf::Event::KeyPressed)
        {
            //win.pollEvent(event);
            if(event.type == sf::Event::Closed)
            {
                //win.close();
                //break;
            }
        }




        //flood_fill_aggressively(img2, x, y);


        rt = img2;

        for(int j=0; j<img.getSize().y; j++)
        {
            for(int i=0; i<img.getSize().x; i++)
            {
                memoise[j][i].clear();
            }
        }


        while(!mouse.isButtonPressed(sf::Mouse::Left))
        {
            win.pollEvent(event);
            if(event.type == sf::Event::Closed)
            {
                win.close();
                return 0;
            }
        }

        int x = mouse.getPosition(win).x/scale;
        int y = mouse.getPosition(win).y/scale;

        auto v = straight_line_detect(memoise, rt, 2.0f, 0, x, y);

        for(auto& i : v)
        {
            rt.setPixel(i.x, i.y, sf::Color(255, 0, 0));
        }

        tex.loadFromImage(rt);
        spr.setTexture(tex);
        spr.setPosition(0,0);

        win.draw(spr);

        //draw(win, img2, 0, img2.getSize().y*0*8);
        win.display();


    }

    return 0;
}
